function testSonar()
	repeat
		write("Front: " .. sonarReading(1), 1)
		write("Right: " .. sonarReading(2), 2)
		write("Left:  " .. sonarReading(3), 3)
	until false
end

function seeObject(direction, distance)
	return sonarReading(direction) < distance
end

function retreatRight(distance)
	backward(A)
	stop(B)
end

function retreatLeft(distance)
	backward(B)
	stop(A)
end

function advance(side, distance)
	forward(A)
	forward(B)
end

function advance(distance)
	forward(A)
	forward(B)
end

function avoid()
	FRONT = 1
	RIGHT = 2
	LEFT = 3
	DISTANCE = 50

	objFront = seeObject(FRONT, DISTANCE)
	objRight = seeObject(RIGHT, DISTANCE)
	objLeft  = seeObject(LEFT, DISTANCE)
	if(objFront) then
		if (sonarReading(LEFT) > sonarReading(RIGHT)) then
			retreatLeft(DISTANCE)
		else
			retreatRight(DISTANCE)
		end	
	elseif(objRight) then
		advance(RIGHT, DISTANCE)
	elseif(objLeft) then
		advance(LEFT, DISTANCE)
	end

end

function forward()
	FRONT = 1
	DISTANCE = 50

	objFront = seeObject(FRONT, DISTANCE)
	if (objFront) then
		avoid()
	else
		advance(DISTANCE)
	end
end

repeat
	avoid()
until isPressed(ESCAPE)



